/***************************************************************
 * Name:      main.cpp
 * Purpose:   fanuc mimic software
 * Author:    Riku Maikola (rpm@ac-mapa.org)
 * Created:   2014-04-01 - 2014-04-21
 * Copyright: Riku Maikola (rpm.ac-mapa.org)
 * License:
 **************************************************************/
#define PI 3.1415926535897932385
#define TAU 6.2831853071795864769
#define DegToRad 0.017453293
#define RadToDeg 57.2957795

#include <vector>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <cmath>

using namespace std;

class ncSolver
{
private:

    float *V;
    float ToDouble(string);
    bool EVA();

public:
    vector<string> P;                   //PART

    ncSolver(float * var,string text="")
    {
        V = var;
        Parse(text);
    };
    void Parse( string M );

    uint Size();
    string GetString();

    bool IsConstant(uint a);
};

vector<string> LineSolve(string linestring,float *var)
{
    string out;
    ncSolver solve(var);
    solve.Parse(linestring);

    return solve.P;
}

bool ncSolver::IsConstant(uint a)
{
    if(P[a].at(0)>47 && P[a].at(0)<58 )
        return true;
    if(P[a].length()>1 && P[a].at(0)==45)
        return true;

    return false;
}

float ncSolver::ToDouble(string in)
{
    return atof(in.c_str());
};

string ncSolver::GetString()
{
    string text;
    for(uint a=0;a<Size();a++)text += P[a];
    return text;
}

uint ncSolver::Size()
{
    return P.size();
}

void ncSolver::Parse(string M )
{
    P.clear();
    for(uint a=0; a<M.length(); a++)
    {
        //Comment removal
        if(M[a]==40)
        {
            a++;
            string TEXT;
            while(M[a]!=41)
            {
                if(a>=M.length())return;
                TEXT += M[a];
                a++;
            }
            //P.push_back(TEXT);
            continue;
        }


        if(((M[a]>34) && (M[a]<48)) || (((M[a]==91) || (M[a]==93)) || (M[a]==61)))
        {
            P.push_back(  M.substr(a,1) );
            continue;
        }

        //numbers
        if(((M[a]>45) && (M[a]<58)))
        {
            string TEXT;
            while(((M[a]>45) && (M[a]<58)) && M[a]!=47)
            {
                TEXT += M[a];
                a++;
            }
            a--;
            P.push_back(TEXT);
            continue;
        }

        //Text
        if( ((M[a]>64) && (M[a]<91)) || ((M[a]>96) && (M[a]<123)) || (M[a]==95))
        {
            string TEXT;
            while( ((M[a]>64) && (M[a]<91)) || (((M[a]>96) && (M[a]<123)) || (M[a]==95)))
            {
                TEXT += M[a];
                a++;
            }
            a--;
            P.push_back(TEXT);
            continue;
        }
    }
    EVA();          //evaporite..
}


bool ncSolver::EVA()
{
start:
    //cout << "in-> " << GetString() << "\n";
    //MAKROS VARIABLE HANDLER
    for(uint a=0; a<Size(); a++)
    {
        if(P[a]=="#" && IsConstant(a+1))
        {
            if(Size()>(2+a))if(P[a+2]=="=")continue;
            P[a] = to_string(V[int(ToDouble(P[a+1]))]);
            P.erase(P.begin()+a+1,P.begin()+a+2);
            continue;
        }
    }
    //
    if(Size()>4)
    {
        if(((P[0]=="#")&&IsConstant(1))&&(P[2]=="="))
        {
            string TEXT;
            for(uint a=3; a<Size(); a++)TEXT += P[a];
            P.erase(P.begin()+3,P.begin()+Size());
            ncSolver SULKU(V,TEXT);
            for(uint b=0;b<SULKU.Size();b++)P.push_back( SULKU.P[b] );
            goto start;
        }
    }

    //substring evas with []
    for(uint a=0; a<Size(); a++)
    {
        if(P[a]=="[")
        {
            //int sulkuja=0;
            string TEXT;
            int alku=0;
            while(P[a]!="]")
            {
                if( P[a]=="[")alku=a;
                if(!(P[a]=="["))TEXT += P[a];
                a++;
                if(a>Size())return false;
            }
            P.erase(P.begin()+alku,P.begin()+a+1);
            ncSolver SULKU(V,TEXT);
            P.insert( P.begin()+alku , SULKU.GetString() );
            goto start;
        }
    }

    //PARSING (-10) minus to value
    if(1<Size())
    {
        if((P[0]=="-") && IsConstant(1))
        {
            P[0] += to_string( ToDouble(P[1]) );
            P.erase(P.begin()+1,P.begin()+2);
        }
    }

    //double minus effect...
    for(uint a=0; a<Size(); a++)
    {
        if((a+2)<Size())
        {
            if((((P[a]=="-"))&&P[a+1]=="-")  && (IsConstant((a+2))) )
            {
                P[a+1] += P[a+2];
                P.erase(P.begin()+a+2,P.begin()+a+3);
                goto start;
            }
            if((!IsConstant(a)) && P[a+1]=="-"  && IsConstant(a+2)     )
            {
                P[a+1] += P[a+2];
                P.erase(P.begin()+a+2,P.begin()+a+3);
                goto start;
            }
        }
    }

    //FUNCTIONS
    for(uint a=0; a<Size(); a++)
    {
        if(Size()==1)
            return true;


        //LOGIC
        if((a+2)<Size())
        {

            if(IsConstant(a) && (P[a+1]=="LE") && IsConstant((a+2)))
            {
                P[a]= to_string( int(ToDouble( P[a])*10000) <= int(ToDouble(P[a+2])*10000));
                P.erase(P.begin()+a+1,P.begin()+a+3);
                goto start;
            }

            if(IsConstant(a) && (P[a+1]=="EQ") && IsConstant((a+2)))
            {
                P[a]= to_string( int(ToDouble( P[a])*10000) == int(ToDouble(P[a+2])*10000));
                P.erase(P.begin()+a+1,P.begin()+a+3);
                goto start;
            }

            if(IsConstant(a) && (P[a+1]=="LT") && IsConstant((a+2)))
            {
                P[a]= to_string( int(ToDouble( P[a])*10000) < int(ToDouble(P[a+2])*10000));
                P.erase(P.begin()+a+1,P.begin()+a+3);
                goto start;
            }
            if(IsConstant(a) && (P[a+1]=="GT") && IsConstant((a+2)))
            {
                P[a]= to_string( int(ToDouble( P[a])*10000) > int(ToDouble(P[a+2])*10000));
                P.erase(P.begin()+a+1,P.begin()+a+3);
                goto start;
            }

        }


        if((a+1)<Size())
        {
            if((P[a]=="SIN") && IsConstant(a+1))
            {
                P[a]= to_string( sin(DegToRad*ToDouble(P[a+1])));
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }
            if((P[a]=="ASIN") && IsConstant(a+1))
            {
                 P[a]= to_string( asin(ToDouble(P[a+1]))*RadToDeg);
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }

            if((P[a]=="COS") && IsConstant(a+1))
            {
                P[a]= to_string( cos(DegToRad*ToDouble(P[a+1])));
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }
            if((P[a]=="ACOS") && IsConstant(a+1))
            {
                P[a]= to_string( acos(ToDouble(P[a+1]))*RadToDeg);
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }
            if((P[a]=="TAN") && IsConstant(a+1))
            {
                P[a]= to_string( tan(DegToRad*ToDouble(P[a+1])));
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }

            if((P[a]=="ATAN") && IsConstant(a+1))
            {
                P[a]= to_string( atan(ToDouble(P[a+1]))*RadToDeg);
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }

            if((P[a]=="FUP") && IsConstant(a+1))
            {
                P[a]= to_string( ceil(ToDouble(P[a+1])));
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }

            if((P[a]=="SQRT") && IsConstant(a+1))
            {
                P[a]= to_string( sqrt(ToDouble(P[a+1])));
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }

            if((P[a]=="ABS") && IsConstant(a+1))
            {
                P[a]= to_string( abs(ToDouble(P[a+1])));
                P.erase(P.begin()+a+1,P.begin()+a+2);
                goto start;
            }

        }
    }
    //
    for(uint a=0; a<Size(); a++)
    {
        if(Size()==1)
            return true;

        if((a+2)<Size())
        {
            if(IsConstant(a) && IsConstant(a+2))
            {
                if(P[a+1]=="*")
                {
                    P[a]=to_string(ToDouble(P[a]) * ToDouble(P[a+2]));
                    P.erase(P.begin()+a+1,P.begin()+a+3);
                    goto start;
                }
                if(P[a+1]=="/")
                {
                    P[a]=to_string(ToDouble(P[a]) / ToDouble(P[a+2]));
                    P.erase(P.begin()+a+1,P.begin()+a+3);
                    goto start;
                }
            }
        }
    }
    // +-
    for(uint a=0; a<Size(); a++)
    {
        if(Size()==1)
            return true;

        if((a+2)<Size())
        {
            if(IsConstant(a) && IsConstant(a+2))
            {
                if(P[a+1]=="+")
                {
                    P[a]=to_string(ToDouble(P[a]) + ToDouble(P[a+2]));
                    P.erase(P.begin()+a+1,P.begin()+a+3);
                    goto start;
                }
                if(P[a+1]=="-")
                {
                    P[a]=to_string(ToDouble(P[a]) - ToDouble(P[a+2]));
                    P.erase(P.begin()+a+1,P.begin()+a+3);
                    goto start;
                }
            }
        }
    }

    //FINISH SET VALUE TO ARRAY.........
    if( (Size()==4 && P[0]=="#") && ((IsConstant(1) && (P[2]=="=") && IsConstant(3))) )
    {
        V[ int(ToDouble(P[1]))] = ToDouble(P[3]);
        P.clear();
        return true;
    }

return true;
}

//******************* SIM *************************//
//****               ROTATE
//*************************************************//
#define ROTATE_X 1001
#define ROTATE_Y 1002
#define ROTATE_Z 1003
#define ROTATE_ANGLE 1004

//FANUC
#define MacroAlarm 3000 //
#define MacroAlarmMessage 3006 //
//time information
#define TimeAfterPowerOn 3001
#define TimeCycleLampOn 3002
#define TimeDAY 3011
#define TimeHoursMinutesSeconds 3012

//Automatic operation control
#define ControlState 3003
#define ControlStateFeed 3004

//Setting
#define Settings 3005

//mirror image protected
#define MirrorImage 3007 //Number of machined parts 0000001=fist axis jne...

//PERSONAL NOT FANUC
#define MotionDist 3500
#define MotionTime 3501
#define RapidDist 3502
#define RunTime 3503
#define CurrentFeed 3504
#define CurrentTool 3505
#define CurrentSpindle 3506
//NUMBER OF MACHINED PARTS can be read and written.
#define CompletionNumber 3901 //Number of machined parts
#define TargetNumber 3902 //Number of required parts

//modal information
#define Group1 4001 //G00,G01,G02,G03,G33
#define Group2 4002 //G17,G18,G19
#define Group3 4003 //G90,G91
#define Group4 4004 //
#define Group5 4005 //G94,G95
#define Group6 4006 //G20,G21
#define Group7 4007 //G40,G41,G42
#define Group8 4008 //G43,G44,G49
#define Group9 4009 //G73,G74,G76,G80-G89
#define Group10 4010 //G98,G99
#define Group11 4011 //G50,G51
#define Group12 4012 //G66,G67
#define Group13 4013 //G69,G97
#define Group14 4014 //G54-G59
#define Group15 4015 //G61-G64
#define Group16 4016 //G68,G69
#define Group22 4022 //

#define Acode 4101 //A
#define Bcode 4102 //B code
#define Ccode 4103 //C
#define Dcode 4104 //D code
#define Ecode 4105 //E
#define Fcode 4106 //F code
#define Gcode 4107 //G
#define Hcode 4108 //H
#define Icode 4109 //I
#define Jcode 4110 //J code
#define Kcode 4111 //K code
#define Lcode 4112 //L
#define Mcode 4113 //M
#define Ncode 4114 //N code
#define Ocode 4115 //O code
#define sequencenumber 4114 //
#define programnumber 4115 //
#define Pcode 4116 //P number of currenty selected additional workpiece coordninate system
#define Qcode 4117 //Q
#define Rcode 4118 //R code
#define Scode 4119 //S code
#define Tcode 4120 //T
#define Ucode 4121 //U
#define Vcode 4122 //V code
#define Wcode 4123 //W
#define Xcode 4124 //X
#define Ycode 4125 //Y
#define Zcode 4126 //Z

//********************//
#define Kcanned 4201 //X
#define Rcanned 4202 //Y
#define Zcanned 4203 //Z

#define BEP_X 5001 //X Block end point,Workpiece coordinate system.
#define BEP_Y 5002 //Y Block end point,Workpiece coordinate system.
#define BEP_Z 5003 //Z Block end point,Workpiece coordinate system.
#define BEP_A 5004 //A Block end point,Workpiece coordinate system.

//CENTER POSITION.......rpm
#define BEP_I 5005 //I Block arc center point,Workpiece coordinate system.
#define BEP_J 5006 //J Block arc center point,Workpiece coordinate system.
#define BEP_K 5007 //K Block arc center point,Workpiece coordinate system.
#define BEP_R 5008 //R Block arc center point,Workpiece coordinate system.

//Current position CAN ONLY BE READ
#define CPM_X 5021 //X Current position,Machine coordinate system.
#define CPM_Y 5022 //Y Current position,Machine coordinate system.
#define CPM_Z 5023 //Z Current position,Machine coordinate system.
#define CPM_A 5024 //A Current position,Machine coordinate system.

#define CP_X 5041 //X Current position,Workpiece coordinate system.
#define CP_Y 5042 //Y Current position,Workpiece coordinate system.
#define CP_Z 5043 //Z Current position,Workpiece coordinate system.
#define CP_A 5044 //A Current position,Workpiece coordinate system.

#define SSP_X 5061 //X Skip signal position,Workpiece coordinate system.
#define SSP_Y 5062 //Y Skip signal position,Workpiece coordinate system.
#define SSP_Z 5063 //Z Skip signal position,Workpiece coordinate system.
#define SSP_A 5064 //A Skip signal position,Workpiece coordinate system.

#define TLOV_X 5081 //X Tool offset value.  but udes for zero point transfer
#define TLOV_Y 5082 //Y
#define TLOV_Z 5083 //Z
#define TLOV_A 5084 //A

#define DSP_X 5101 //X Deviated servo position
#define DSP_Y 5102 //Y
#define DSP_Z 5103 //Z
#define DSP_A 5104 //A

//Workpiece coordinate system compensation values (workpiece zero point offset values)
//CAN BE READ AND WRITTEN
#define EXT_X 5201 //X external workpiece zero point offset value
#define EXT_Y 5202 //Y external workpiece zero point offset value
#define EXT_Z 5203 //Z external workpiece zero point offset value
#define EXT_A 5204 //A external workpiece zero point offset value

#define G54_X 5221 //X workpiece zero point offset value
#define G54_Y 5222 //Y workpiece zero point offset value
#define G54_Z 5223 //Z workpiece zero point offset value
#define G54_A 5224 //A workpiece zero point offset value

#define G55_X 5241 //X workpiece zero point offset value
#define G55_Y 5242 //Y workpiece zero point offset value
#define G55_Z 5243 //Z workpiece zero point offset value
#define G55_A 5244 //A workpiece zero point offset value

#define G56_X 5261 //X workpiece zero point offset value
#define G56_Y 5262 //Y workpiece zero point offset value
#define G56_Z 5263 //Z workpiece zero point offset value
#define G56_A 5264 //A workpiece zero point offset value

#define G57_X 5281 //X workpiece zero point offset value
#define G57_Y 5282 //Y workpiece zero point offset value
#define G57_Z 5283 //Z workpiece zero point offset value
#define G57_A 5284 //A workpiece zero point offset value

#define G58_X 5301 //X workpiece zero point offset value
#define G58_Y 5302 //Y workpiece zero point offset value
#define G58_Z 5303 //Z workpiece zero point offset value
#define G58_A 5304 //A workpiece zero point offset value

#define G59_X 5321 //X workpiece zero point offset value
#define G59_Y 5322 //Y workpiece zero point offset value
#define G59_Z 5323 //Z workpiece zero point offset value
#define G59_A 5324 //A workpiece zero point offset value

#define G541P_X 7001 //X NEXT-- P2 = 7001+20
#define G541P_Y 7002 //Y workpiece zero point offset value
#define G541P_Z 7003 //Z workpiece zero point offset value
#define G541P_A 7004 //A workpiece zero point offset value

//Tool compensation values
#define H_Geometric_compensation 11001 //1--400
#define H_Wear_compensation 10001		//

#define D_Geometric_compensation 13001 //
#define D_Wear_compensation 12001		//

//*******************************************************************************//
//***
//*******************************************************************************//
//#include <vector>
//#include <stdlib.h>
#include <fstream>
#include <cmath>

using namespace std;
struct point
{
    char g;     //G-komento
    char t;     //työkalu
    float s;    //spindle speed
    float f;    //feed mm/min
    float x,y,z;//position;
};
//*************************//
//**    CONTROL CLASS    **//
//*************************//
class mNC
{
private:
    float V[20000];   //Machine memory array

    //double time;
    string MSG;
    vector<string> C;      //configure
    vector<string> R;      //PROGRAM.ALL
    vector<point> I;       //RUN DATA LOG

    void Run(vector<string> rows);
    void SubRun(vector<string> rows);   //Makrorun

    void G0(float,float,float);
    void G1(float,float,float);
    //void G2(double,double,double,double,double,double);
    //void G3(double,double,double,double,double,double);

public:
     mNC(string a);
    ~mNC();

    //bool Config();
    void CYCLE(vector<string> rows);
    vector<string> LoadFile(string);
    vector<string> LoadProg(string);
    void Add(char g,char t,float s,float f,float x,float y,float z);

};

float Distance(float x1,float y1,float z1,float x2,float y2,float z2);

mNC::mNC(string dir)
{
     string line;

    //project spesific config
    //0 line global simulator array/program.all directory
    ifstream configfile(dir+"/sim.config");
    if (configfile.is_open())
    {
        for(uint a=0;a<100;a++)if(getline(configfile,line))C.push_back(line);
             configfile.close();
    }
    else
    {
        MSG += "NO CONFIG FILE FOUND\n";
        cout << dir << "sim.config" << " not found\n";
        return;
    }

    //PROGRAM.ALL
    ifstream programfile(C[1]);
    if (programfile.is_open())
    {
        while(getline(programfile,line))R.push_back(line);
        programfile.close();
    }
    else
    {
        MSG += "PRAGRAM.ALL FILE NOT FOUND\n";
        cout << C[1] << " not found\n";
        return;
    }


    //CONFIG ARRAY LOAD
    ifstream codefile(C[0]);
    if (codefile.is_open())
    {
        //for(uint a=0;a<10;a++)if(getline(codefile,line))C.push_back(line);
        for(uint a=0;a<19999;a++)if(getline(codefile,line))V[a]=atof(line.c_str());
        codefile.close();
        Run(LoadFile(dir+"/CODE"));
    }
    else
    {
        MSG += "NO ARRAY FILE FOUND\n";
         cout << C[0] << " not found\n";;
        return;
    }

    //AAAAAAAAAAA
    ofstream myfile;
    myfile.open(dir+"/sim.log");
    if (myfile.is_open())
    {
        myfile << MSG ;
        myfile.close();
    }
    //BBBBBBBBBBBB
    /*myfile.open(C[2],ios::out | ios::trunc | ios::binary);
    if (myfile.is_open())
    {
        myfile.write( (char*)&I[0] , I.size()*sizeof(point) );
        myfile.close();
    }*/

    //SAVE POINT STREAM
    streampos size;
    string file = dir + "/sim.out";

    myfile.open (file.c_str(),ios::out | ios::trunc | ios::binary);
    if(myfile.is_open())
    {
        myfile.write( (char*)&I[0] , I.size()*sizeof(point) );
        myfile.close();
    }
}

mNC::~mNC(){}

//LOADFILE
vector<string> mNC::LoadFile(string filename)
{
    //*********************
    vector<string> rows;

    //PARSE ROWS***********
    string line;
    ifstream codefile(filename.c_str());
    if (codefile.is_open())
    {
        while (getline(codefile,line))rows.push_back(line);
        codefile.close();
        return rows;
    }
    MSG += "CAN'T LOAD FILE="+filename+"\n";
    return rows;
}
//----------//
// LOADFILE //
vector<string> mNC::LoadProg(string number)
{

    string line;
    vector<string> lrows;

    lrows.push_back("%");
    for(uint a=0 ; a<R.size() ; a++)
    {
        //std::cout << << " hire\n";


        if(R[a].find("O"+number)!=std::string::npos)
        {
            lrows.push_back(R[a]);
            for( uint b=a+1 ; b<R.size() ;b++)
            {
                if(R[b][0]==79)break;

                lrows.push_back(R[b]);
            }
        }
    }
    lrows.push_back("%");
    return lrows;
}




void mNC::Run(vector<string> rows)
{
    int LOOPLIMIT=1000000;       //maximum loop break
    size_t rp=-1;                //row number

    //return if it has small letters
    /*    for(size_t a=0; a<code.Len(); a++)
        {
            if(code[a]>93)
            {
                MSG << wxString::Format( _("\n IS NOT G code\n"));
                return;
            }
        }

        if(code.Contains(_("MSG::")))return;*/

    //wxArrayString rows = wxStringTokenize(code, wxT("\n"),wxTOKEN_STRTOK);  //parse rows



    while(true)
    {
        rp++;
        //LIMITERS...
        if(rows.size()<=rp)MSG = "ROW LIMIT EXCEEDED";
        LOOPLIMIT--;
        if(LOOPLIMIT==0)MSG = "LOOPLIMIT";
        if(!(MSG==""))break;




        //*************
        vector<string> P = LineSolve(rows[rp],V);

        if(P.size()==0)continue;

        //END ROUTINES
        //WHILE
        if(P.size()==4)
        {
            if((P[0]=="WHILE"))
            {
                if(P[1]=="1")continue;
                if(P[1]=="0")
                {
                    while(true)
                    {
                        rp++;
                        if(rp>rows.size())
                        {
                            MSG = "CAN'T FIND END BLOCK";
                            break;
                        }
                        if (rows[rp].find("END"+P[3])!=std::string::npos)break;
                    }
                    continue;
                }
            }
        }

        //END STATEMENT-->RETURN TO DO
        if(P.size()==2)
        {
            if(P[0]=="END")
            {
                while(true)
                {
                    rp--;
                    if(rp<1)
                    {
                        MSG = "CAN'T FIND DO BLOCK";
                        break;
                    }
                    if (rows[rp].find("DO"+P[1])!=std::string::npos)
                    {
                        rp--;
                        break;
                    }
                }
                continue;
            }
        }

        if(P.size()==7)
        {
            if((P[0]=="IF") && P[1]=="1" && (P[2]=="THEN"))
            {
                //P.erase(P.begin()+1,P.begin()+3);
                string dta;
                for(uint alt=3; alt<P.size(); alt++)dta += P[alt];
                vector<string> out = LineSolve(dta,V);
                continue;//cout << out.size() << "\n";
            }
            if((P[0]=="IF") && P[1]=="0" && (P[2]=="THEN"))continue;
        }
        if(P.size()==4)
        {
            if((P[0]=="IF")&&(P[1]=="1")&&(P[2]=="GOTO"))P.erase(P.begin(),P.begin()+2);
        }


        if(P.size()==2)
        {
            if(P[0]=="GOTO")
            {
                rp = -1;
                while(true)
                {
                    rp++;
                    if(rp>rows.size())
                    {
                        MSG = "CAN'T FIND N? BLOCK";
                        break;
                    }
                    if(rows[rp].find("N"+P[1])!=std::string::npos)
                    {
                        rp--;
                        break;
                    }
                }
                continue;
            }
        }

        for(int a=4101; a<4127; a++)
        {
            V[a] = NAN;
        }

        for(uint a=0; P.size()>a; a++)
        {
            if(P[a]=="%")continue;
            if( P[a].length()>1)   //should not bee over one char
            {
                MSG = "CHARACTER ERROR=" + P[a];
            }

            if(P[a]=="G")
            {
                a++;
                float G=atof( P[a].c_str() );

                if(G==0 || G==1 || G==2 || G==3 || G==33)V[Group1]=G;       //Group1
                else if(G==17 || G==18 || G==19)V[Group2]=G;                     //Group2
                else if(G==90 || G==91)V[Group3]=G;                              //Group3
                //Group4
                else if(G==94 || G==95)V[Group5]=G;                              //Group5 G94,G95
                else if(G==20 || G==21)V[Group6]=G;                              //Group6 G20,G21
                else if(G==40 || G==41 || G==42)V[Group7]=G;                     //Group7 G40,G41,G42
                else if(G==43 || G==44 || G==49)V[Group8]=G;                     //Group8 G43,G44,G49
                else if((G==73 || G==74 || G==76) || (G>79 && G<90))V[Group9]=G; //Group9 G73,G74,G76,G80-G89
                else if(G==98 || G==99)V[Group10]=G;                             //Group10 G98,G99
                else if(G==50 || G==51)V[Group11]=G;                             //Group11 G50,G51
                else if(G==66 || G==67)V[Group12]=G;                             //Group12 G66,G67
                else if(G==69 || G==97)V[Group13]=G;                             //Group13 G69,G97
                else if(G>53 && G<60)V[Group14]=G;                               //Group14 G54-G59
                else if(G>60 && G<65)V[Group15]=G;                               //Group15 G61-G64
                else if(G==68 || G==69)V[Group16]=G;                             //Group16 G68,G69
                else
                {
                    V[Gcode]=G;
                }
                continue;
            };


            //THESE SHOULD BE ONE TIMERS
            if(P[a]=="A")
            {
                a++;
                V[Acode] = atof( P[a].c_str() );
                continue;
            }


            if(P[a]=="B")
            {
                a++;
                V[Bcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="C")
            {
                a++;
                V[Ccode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="D")
            {
                a++;
                V[Dcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="E")
            {
                a++;
                V[Ecode] = atof( P[a].c_str() );
                continue;
            }
            if(P[a]=="F")
            {
                a++;
                V[Fcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="H")
            {
                a++;
                V[Hcode] = atof( P[a].c_str() );
                continue;
            }


            //DOT
            if(P[a]=="I")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Icode] = atof( P[a].c_str() )*kerroin;

                continue;
            }
            if(P[a]=="J")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Jcode] = atof( P[a].c_str() )*kerroin;

                continue;
            }
            if(P[a]=="K")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Kcode] = atof( P[a].c_str() )*kerroin;

                continue;

            }

            if(P[a]=="L")
            {
                a++;
                V[Lcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="M")
            {
                a++;
                V[Mcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="N")
            {
                a++;
                V[Ncode] = atof(P[a].c_str());
                continue;
            }
            if(P[a]=="O")
            {
                a++;
                V[Ocode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="P")
            {
                a++;
                V[Pcode] = atof(P[a].c_str() );
                continue;
            }

            if(P[a]=="Q")
            {
                a++;
                V[Qcode] = atof( P[a].c_str() );
                continue;
            }


            if(P[a]=="R")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Rcode] = atof( P[a].c_str() )*kerroin;

                continue;

            }



            if(P[a]=="S")
            {
                a++;
                V[Scode] = atof( P[a].c_str() );
                continue;
            }
            if(P[a]=="T")
            {
                a++;
                V[Tcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="U")
            {
                a++;
                V[Ucode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="V")
            {
                a++;
                V[Vcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="W")
            {
                a++;
                V[Wcode] = atof( P[a].c_str() );
                continue;
            }

            if(P[a]=="X")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Xcode] = atof( P[a].c_str() )*kerroin;
                continue;
            }

            if(P[a]=="Y")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Ycode] = atof( P[a].c_str() )*kerroin;
                continue;
            }
            if(P[a]=="Z")
            {
                float kerroin=0.001;
                a++;
                if(P[a].find(".")!=std::string::npos)kerroin=1;
                V[Zcode] = atof( P[a].c_str() )*kerroin;

                continue;
            }


            //ERR << _("ERROR WRONG CHARACTER") << P[a];
            //break;

        };

        if(!(MSG==""))break;


        //G43 -- ToolOffest --
        if(int(V[Group8])==43)
        {
            V[Group8]=0;
            continue;
        }

        //G43 -- ROTATE MATRIX --
        if(int(V[Group16])==68)
        {
            V[Group16]=1;                     //TRANSFORM LOCATION TO CORRECT PLACE
            V[ROTATE_ANGLE] = V[Rcode]*DegToRad;

            V[ROTATE_X] = V[Xcode];
            V[ROTATE_Y] = V[Ycode];
            V[ROTATE_Z] = V[Zcode];

            continue;
        }

        if(int(V[Group16])==69)
        {
            continue;
        }

        //G92 -- CHANCE --
        if(int(V[Gcode])==92)
        {
            V[Gcode]=NAN;
            V[TLOV_X] = V[int(G54_X + ((V[Group14]-54)*20))];
            V[TLOV_Y] = V[int(G54_Y + ((V[Group14]-54)*20))];
            V[TLOV_Z] = V[int(G54_Z + ((V[Group14]-54)*20))];
            continue;
        }
        if(int(V[Gcode])==28)
        {
            V[Gcode]=NAN;
            V[TLOV_X] = 0;
            V[TLOV_Y] = 0;
            V[TLOV_Z] = 0;
        }

        //MAKROCALL G65
        if(int(V[Gcode])==65)
        {
            // SubRun( LoadFile(wxString::Format(wxT("%1.0f"),V[Pcode])) );
            continue;
        }

        if(int(V[Mcode])==30)
        {
            MSG = "NORMAL EXIT\n";
            break;
        }

        if(int(V[Mcode])==99)
        {
            MSG == "";
            break;
        }
        /*
        if(int(V[Gcode])==700)
        {
            SubRun(LoadFile(_("9010")));
            continue;
        }
        if(int(V[Gcode])==701)
        {
            SubRun(LoadFile(_("9011")));
            continue;
        }
        if(int(V[Gcode])==702)
        {
            SubRun(LoadFile(_("9012")));
            continue;
        }
        if(int(V[Gcode])==703)
        {
            SubRun(LoadFile(_("9013")));
            continue;
        }
        if(int(V[Gcode])==704)
        {
            SubRun(LoadFile(_("9014")));
            continue;
        }
        if(int(V[Gcode])==705)
        {
            SubRun(LoadFile(_("9015")));
            continue;
        }
        if(int(V[Mcode])==150)
        {
            SubRun(LoadFile(_("9001")));
            continue;
        }
        */
        if(int(V[Mcode])==100)
        {
            SubRun(LoadProg("9020"));
            continue;
        }
        if(int(V[Mcode])==101)
        {
            SubRun(LoadProg("9021"));
            continue;
        }
        if(int(V[Mcode])==102)
        {
            SubRun(LoadProg("9022"));
            continue;
        }
        /*
        if(int(V[Mcode])==200)
        {
            SubRun(LoadFile(_("9023")));
            continue;
        }
        if(int(V[Mcode])==201)
        {
            SubRun(LoadFile(_("9024")));
            continue;
        }
        if(int(V[Mcode])==202)
        {
            SubRun(LoadFile(_("9025")));
            continue;
        }
        */

        if(V[Group9]==76)
        {
            if(!isnan(V[Kcode])) V[Kcanned] = V[Kcode];
            if(!isnan(V[Rcode])) V[Rcanned] = V[Rcode];
            if(!isnan(V[Zcode])) V[Zcanned] = V[Zcode];
            if(isnan(V[Ycode])&&isnan(V[Xcode]))continue;  //JUMP OVER IF NO CHANGE
            if(V[Kcanned]<1)V[Kcanned]=1;

            for(int a=0; V[Kcanned]>a; a++)
            {
                G0(V[Xcode],V[Ycode],NAN);
                G0(NAN,NAN, V[Rcanned] );
                G1(NAN,NAN, V[Zcanned] );
                G0(NAN,NAN, V[Rcanned] );
            }
            V[Kcanned]=1;
            continue;
        }


        if(V[Group9]==81)
        {
            if(!isnan(V[Kcode])) V[Kcanned] = V[Kcode];
            if(!isnan(V[Rcode])) V[Rcanned] = V[Rcode];
            if(!isnan(V[Zcode])) V[Zcanned] = V[Zcode];
            if(isnan(V[Ycode])&&isnan(V[Xcode]))continue;  //JUMP OVER IF NO CHANGE
            if(V[Kcanned]<1) V[Kcanned]=1;

            for(int a=0; V[Kcanned]>a; a++)
            {
                G0(V[Xcode],V[Ycode],NAN);
                G0(NAN,NAN, V[Rcanned] );
                G1(NAN,NAN, V[Zcanned] );
                G0(NAN,NAN, V[Rcanned] );
            }
            V[Kcanned]=1;
            continue;
        }

        if(V[Group9]==84)
        {
            if(!isnan(V[Kcode])) V[Kcanned] = V[Kcode];
            if(!isnan(V[Rcode])) V[Rcanned] = V[Rcode];
            if(!isnan(V[Zcode])) V[Zcanned] = V[Zcode];
            if(isnan(V[Ycode])&&isnan(V[Xcode]))continue;  //JUMP OVER IF NO CHANGE
            if(V[Kcanned]<1)V[Kcanned]=1;

            for(int a=0; V[Kcanned]>a; a++)
            {
                G0(V[Xcode],V[Ycode],NAN);
                G0(NAN,NAN, V[Rcanned] );
                G1(NAN,NAN, V[Zcanned] );
                G1(NAN,NAN, V[Rcanned] );
            }
            V[Kcanned]=1;
            continue;
        }

        if(!isnan(V[Scode])) V[CurrentSpindle]=V[Scode];
        if(!isnan(V[Fcode])) V[CurrentFeed]=V[Fcode];
        if(!isnan(V[Tcode])) V[CurrentTool]=V[Tcode];
        if(V[Group9]==80)
        {


            if(!isnan(V[Rcanned])) V[Rcanned] = NAN;
            if(!isnan(V[Kcanned])) V[Kcanned] = NAN;
            if(!isnan(V[Zcanned])) V[Zcanned] = NAN;


            if( (!isnan(V[Xcode])) ||  (!isnan(V[Ycode])) ||  (!isnan(V[Zcode])) || (!isnan(V[Icode])) ||  (!isnan(V[Jcode])) ||  (!isnan(V[Kcode])) )
            {
                if(!isnan(V[Xcode]))V[CP_X] = V[Xcode];
                if(!isnan(V[Ycode]))V[CP_Y] = V[Ycode];
                if(!isnan(V[Zcode]))V[CP_Z] = V[Zcode] ;
                /*
                                //execute ROTATE location manipulation experimental.........
                                if(int(V[Group16])==1)
                                {
                                    if(isnan(V[Xcode]))V[Xcode]=V[CP_X];
                                    if(isnan(V[Ycode]))V[Ycode]=V[CP_Y];
                                    if(isnan(V[Zcode]))V[Zcode]=V[CP_Z];

                                    mPoint T;
                                    T.SetValue(mP(V[Xcode],V[Ycode],V[Zcode]));

                                    //T -= mP(V[ROTATE_X],V[ROTATE_Y],V[ROTATE_Z]);
                                    T.RotateXY(V[ROTATE_ANGLE]);
                                    //T += mP(V[ROTATE_X],V[ROTATE_Y],V[ROTATE_Z]);
                                    V[Xcode] = T.x;
                                    V[Ycode] = T.y;
                                    V[Zcode] = T.z;
                                }
                */
                if((V[Group1]==2 || V[Group1]==3) && (!isnan(V[Rcode])))
                {
                    if(isnan(V[Xcode]))V[Xcode]=V[CPM_X]-V[TLOV_X];
                    if(isnan(V[Ycode]))V[Ycode]=V[CPM_Y]-V[TLOV_Y];
                    if(isnan(V[Zcode]))V[Zcode]=V[CPM_Z]-V[TLOV_Z];

                    /*mPoint p1,p2,cp;
                    p1.SetValue(V[CPM_X]-V[TLOV_X],V[CPM_Y] -V[TLOV_Y],V[CPM_Z]-V[TLOV_Z]);
                    p2.SetValue(V[Xcode],V[Ycode],V[Zcode]);
                    if(V[Group1]==2)cp.SetCWCircleCenter(p1,p2,V[Rcode]);
                    if(V[Group1]==3)cp.SetCCWCircleCenter(p1,p2,V[Rcode]);
                    V[Icode]=cp.x-p1.x;   //CENTER POSITION....
                    V[Jcode]=cp.y-p1.y;
                    V[Kcode]=cp.z-p1.z;*/
                }
                if(V[Group1]==0)G0(V[Xcode],V[Ycode],V[Zcode]);
                if(V[Group1]==1)G1(V[Xcode],V[Ycode],V[Zcode]);
                //if(V[Group1]==2)G2(V[Xcode],V[Ycode],V[Zcode],V[Icode],V[Jcode],V[Kcode]);
                //if(V[Group1]==3)G3(V[Xcode],V[Ycode],V[Zcode],V[Icode],V[Jcode],V[Kcode]);
            };
        };
    };
    //cout << MSG << "\n";
}

//
void mNC::SubRun(vector<string> rows)
{
    V[Gcode]=NAN;
    //SAVE LOCAL VARIABLES
    float BUF[35];
    for(int a=0; a<35; a++)BUF[a]=V[a];

    //SET VARIABLES
    V[1]=V[Acode];
    V[2]=V[Bcode];
    V[3]=V[Ccode];
    V[7]=V[Dcode];
    V[8]=V[Ecode];
    V[9]=V[Fcode];
    V[11]=V[Hcode];

    V[4]=V[Icode];
    V[5]=V[Jcode];
    V[6]=V[Kcode];
    V[13]=V[Mcode];
    V[17]=V[Qcode];
    V[18]=V[Rcode];
    V[19]=V[Scode];

    V[20]=V[Tcode];
    V[21]=V[Ucode];
    V[22]=V[Vcode];
    V[23]=V[Wcode];
    V[24]=V[Xcode];
    V[25]=V[Ycode];
    V[26]=V[Zcode];
    //SET VARIABLES BACK
    Run(rows);
    for(int a=0; a<35; a++)V[a]=BUF[a];

}

//G0
void mNC::G0(float X,float Y,float Z)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    float mx = X-V[CPM_X];
    float my = Y-V[CPM_Y];
    float mz = Z-V[CPM_Z];
    float D = sqrt((mx*mx)+(my*my)+(mz*mz));

    float Dp = 12000/5000;     //step lenght
    double div = Dp/D;

    mx *= div;
    my *= div;
    mz *= div;

    //**************
    for(int a=0;a<int(D/Dp);a++)
    {
        V[CPM_X] += mx;
        V[CPM_Y] += my;
        V[CPM_Z] += mz;

        Add(0,char(V[CurrentTool]),V[CurrentSpindle],V[CurrentFeed],V[CPM_X],V[CPM_Y],V[CPM_Z]);
    }

    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}

//G1
void mNC::G1(float X,float Y,float Z)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    //--------------------
    float mx = X-V[CPM_X];
    float my = Y-V[CPM_Y];
    float mz = Z-V[CPM_Z];
    float D = sqrt((mx*mx)+(my*my)+(mz*mz));
    float Dp = 12000/5000;     //step lenght
    double div = Dp/D;
    mx *= div;
    my *= div;
    mz *= div;

    //--------------------------
    for(int a=0;a<int(D/Dp);a++)
    {
        V[CPM_X] += mx;
        V[CPM_Y] += my;
        V[CPM_Z] += mz;
        Add(1,char(V[CurrentTool]),V[CurrentSpindle],V[CurrentFeed],V[CPM_X],V[CPM_Y],V[CPM_Z]);
    }

    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}
/*
//G2
void mNC::G2(float X,float Y,float Z,float CX,float CY,float CZ)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    if(isnan(CX)) CX = 0;
    if(isnan(CY)) CY = 0;
    if(isnan(CZ)) CZ = 0;

    mPoint P1,P2,CP;
    P2.SetValue(X,Y,Z);
    P1.SetValue(V[CPM_X],V[CPM_Y],V[CPM_Z]);
    CP.SetValue(P1.x+CX,P1.y+CY,P1.z+CZ);

    float a1 = CP.GetDirectionXY(P1);
    float a2 = CP.GetDirectionXY(P2);
    float  r = CP.GetDistanceXY(P1);

    if(!AER(r,CP.GetDistanceXY(P2),0.001))
    {
        printf("RADIUS ERROR G2 i=%f J=%f K=%f %f!=%f\n",CX,CY,CZ,r,CP.GetDistanceXY(P2));
    }

    float arc= 0;

    if(a1<=a2) a2-=TAU;

    float drop;

    if(a2<0)
        arc = a1+(-a2);
    else
        arc = a1-a2;

    V[RunTime] += (arc*r)/V[CurrentFeed];;
    drop = (V[CPM_Z]-Z)/arc*0.1;

    //LOOP
    while(a1>a2)
    {
        a1=a1-0.1;
        V[CPM_Z] -= drop;
        if(a1<a2)a1=a2;


        addP(mP(cos(a1)*r+CP.x,sin(a1)*r+CP.y,V[CPM_Z]) , 2 ,

                        I.at(I.size()-1).ny+(Distance3D(I.at(I.size()-1).x ,

                                               I.at(I.size()-1).y,
                                               I.at(I.size()-1).z,
                                               cos(a1)*r+CP.x,
                                               sin(a1)*r+CP.y,
                                               V[CPM_Z]))/V[CurrentFeed]

                        , V[CurrentTool] ) ;
    }
    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}

//G3
void mNC::G3(float X,float Y,float Z,float CX,float CY,float CZ)
{
    if(isnan(X)) X = V[CPM_X];
    else X=X+V[TLOV_X];
    if(isnan(Y)) Y = V[CPM_Y];
    else Y=Y+V[TLOV_Y];
    if(isnan(Z)) Z = V[CPM_Z];
    else Z=Z+V[TLOV_Z];

    if(isnan(CX)) CX = 0;
    if(isnan(CY)) CY = 0;
    if(isnan(CZ)) CZ = 0;

    mPoint P1,P2,CP;
    P2.SetValue(X,Y,Z);
    P1.SetValue(V[CPM_X],V[CPM_Y],V[CPM_Z]);
    CP.SetValue(P1.x+CX,P1.y+CY,P1.z+CZ);

    float a1 = CP.GetDirectionXY(P1);
    float a2 = CP.GetDirectionXY(P2);

    float  r = CP.GetDistanceXY(P1);
    if(!AER(r,CP.GetDistanceXY(P2),0.001))
    {
        printf("RADIUS ERROR G3 i=%f J=%f K=%f %f!=%f\n",CX,CY,CZ,r,CP.GetDistanceXY(P2));
    };
    if(a1>=a2)a2+=TAU;

    V[RunTime] += ((a2-a1)*r)/V[CurrentFeed];;
    float drop = (V[CPM_Z]-Z)/((a2-a1))*0.1;

    while(a1<a2)
    {
        a1=a1+0.1;
        V[CPM_Z] -= drop;
        if(a1>a2)a1=a2;

        addP(mP( cos(a1)*r+CP.x,sin(a1)*r+CP.y ,V[CPM_Z]) , 3 ,
                        I.at(I.size()-1).ny+(Distance3D(I.at(I.size()-1).x ,
                        I.at(I.size()-1).y,I.at(I.size()-1).z,
                        cos(a1)*r+CP.x,sin(a1)*r+CP.y,
                        V[CPM_Z]))/V[CurrentFeed],V[CurrentTool]
                    );
    }
    V[CPM_X]=X;
    V[CPM_Y]=Y;
    V[CPM_Z]=Z;
}


*/

//
void mNC::Add(char g,char t,float s,float f,float x,float y,float z)
{
    point n;

    n.g = g;
    n.y = t;
    n.s = s;
    n.f = f;
    n.x = x;
    n.y = y;
    n.z = z;

    I.push_back(n);
    return;
}
float Distance(float x1,float y1,float z1,float x2,float y2,float z2)
{
    float x = x1-x2;
    float y = y1-y2;
    float z = z1-z2;
    return sqrt((x*x)+(y*y)+(z*z));
};

//*************************//
int main(int n, char* Args[])
{
    cout << "start simulation\n";
    if(n>0) mNC machine(Args[1]);

    return 0;
}
